//
// Created by rebeater on 3/30/21.
//

#include "RobotDecode.h"
#include "nav_struct.h"
#include <fstream>
#include <iostream>
#include "nav_io.h"

using namespace std;

const double adis16460_ka_g = 0.25e-3;
const double adis16460_kg = 0.005 * _PI / 180.0;


void convert_adi_to_double(ImuRawAdi *adi, ImuData *imu) {
    imu->gpst = adi->time_s;
    imu->gyro[0] = adis16460_kg * adi->gyro[0];
    imu->gyro[1] = adis16460_kg * adi->gyro[1];
    imu->gyro[2] = adis16460_kg * adi->gyro[2];

    imu->acce[0] = adis16460_ka_g * adi->acce[0];
    imu->acce[1] = adis16460_ka_g * adi->acce[1];
    imu->acce[2] = adis16460_ka_g * adi->acce[2];
}

const double  _gyro_scale = 1.0f / 32.767f;
const double _accel_scale = 1.0f / 4096.0f;
void convert_icm_to_double(ImuRawIcm *icm, ImuData *imu) {
    imu->gpst = icm->gpst;
    imu->gyro[0] = _gyro_scale * icm->gyro[0] * _deg;
    imu->gyro[1] = _gyro_scale * icm->gyro[1] * _deg;
    imu->gyro[2] = _gyro_scale * icm->gyro[2] * _deg;

    imu->acce[0] = _accel_scale * icm->acce[0];
    imu->acce[1] = _accel_scale * icm->acce[1];
    imu->acce[2] = _accel_scale * icm->acce[2];
}
void convert_stim_to_imu(ImuRawStim *stim,ImuData *imu){
    imu->gpst = stim->time_s;
    imu->acce[0] = stim->acce[0]*1.0/ (1u << 19u);
    imu->acce[1] = stim->acce[1]*1.0/ (1u << 19u);
    imu->acce[2] = stim->acce[2]*1.0/ (1u << 19u);
    imu->gyro[0] = stim->gyro[0] * _deg/ (1u << 14u);
    imu->gyro[1] = stim->gyro[1] * _deg/ (1u << 14u);
    imu->gyro[2] = stim->gyro[2] * _deg/ (1u << 14u);
}





void RobotDecode::convertGnssToTXT(const char *gnss_path, const char *txt_path) {
    GnssData gnss;
    ifstream ifs(gnss_path,ios::binary);
    ofstream ofs(txt_path);
    while(ifs.good() and !ifs.eof()){
        ifs.read(reinterpret_cast<char *>(&gnss),sizeof(GnssData));
        ofs<<gnss<<'\n';
    }
}

void RobotDecode::convertAdiToTXT(const char *adi_path, const char *txt_path) {
    ImuRawAdi adi;
    ImuData imu;
    ifstream ifs(adi_path,ios::binary);
    ofstream ofs(txt_path);
    while(ifs.good() and !ifs.eof()){
        ifs.read(reinterpret_cast<char *>(&adi),sizeof(ImuRawAdi));
        convert_adi_to_double(&adi,&imu);
        ofs<<imu<<'\n';
    }
}

void RobotDecode::convertRstToNav(const char *rst_path, const char *txt_path) {
    NavOutput rst;
    ifstream ifs(rst_path,ios::binary);
    ofstream ofs(txt_path);
    while(ifs.good() and !ifs.eof()){
        ifs.read(reinterpret_cast<char *>(&rst),sizeof(GnssData));
        ofs<<rst<<'\n';
    }
}

void RobotDecode::convertIcmToTXT(const char *icmpath, const char *txt_path) {
    ImuRawIcm icm;
    ImuData imu;
    ifstream ifs(icmpath,ios::binary);
    ofstream ofs(txt_path);
    while(ifs.good() and !ifs.eof()){
        ifs.read(reinterpret_cast<char *>(&icm),sizeof(ImuRawIcm));
        convert_icm_to_double(&icm,&imu);
            ofs<<imu<<'\n';
    }
}
void RobotDecode::convertVelToTXT(const char *velpath, const char *txt_path) {
   Velocity vel;
    ifstream ifs(velpath,ios::binary);
    ofstream ofs(txt_path);
    while(ifs.good() and !ifs.eof()){
        ifs.read(reinterpret_cast<char *>(&vel),sizeof(Velocity));
        ofs<<vel<<'\n';
    }
}

void RobotDecode::convertStimToTxt(const char *stim_path, const char *txt_path) {
    ImuRawStim stim;
    ImuData  imu;
    ifstream ifs(stim_path,ios::binary);
    ofstream ofs(txt_path);
    while(ifs.good() and !ifs.eof()){
        ifs.read(reinterpret_cast<char *>(&stim),sizeof(ImuRawStim));
        convert_stim_to_imu(&stim,&imu);
        ofs<<imu<< '\n';
    }
}
void RobotDecode::convertEntToTxt(const char *ent_path, const char *txt_path) {
  CameraEvent camera_event;
  ifstream ifs(ent_path,ios::binary);
  ofstream ofs(txt_path);
  while(ifs.good() and !ifs.eof()){
	ifs.read(reinterpret_cast<char *>(&camera_event),sizeof(CameraEvent));
	ofs<<camera_event<< '\n';
  }
}

